﻿using Microsoft.VisualStudio.TestTools.UnitTesting;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace XRoute.PathFinding.Test
{
    [TestClass]
    public class PathTest
    {
        [TestMethod]
        public void TestAddStep()
        {
            var begin = new Path<int>(100);
            var middle = begin.AddStep(101, 1);
            var end = middle.AddStep(102, 2);

            Assert.AreEqual(begin, middle.Tail);
            Assert.AreEqual(middle, end.Tail);
            Assert.AreEqual(1, middle.TotalCost);
            Assert.AreEqual(3, end.TotalCost);

            CollectionAssert.AreEqual(new int[] { 102, 101, 100 }, end.ToArray());
        }

        [TestMethod]
        public void TestFindPath_Dijkstra()
        {
            /*
             * Map:
             *    n3 n8   
             * n1 n2 n6   
             * n4 n5 n7 n9
             */
            var n1 = new Node { cost = 1 };
            var n2 = new Node { cost = 2 };
            var n3 = new Node { cost = 3 };
            var n4 = new Node { cost = 4 };
            var n5 = new Node { cost = 5 };
            var n6 = new Node { cost = 6 };
            var n7 = new Node { cost = 7 };
            var n8 = new Node { cost = 8 };
            var n9 = new Node { cost = 9 };
            var n0 = new Node(); // not in graph

            n1.east = n2; n1.south = n4;
            n2.north = n3; n2.west = n1; n2.east = n6; n2.south = n5;
            n3.east = n8; n3.south = n2;
            n4.north = n1; n4.east = n5;
            n5.north = n2; n5.west = n4; n5.east = n7;
            n6.north = n8; n6.west = n2; n6.south = n7;
            n7.north = n6; n7.east = n5; n7.west = n9;
            n8.west = n3; n8.south = n6;
            n9.east = n7;

            Func<Path<Node>, Node, double> distance = (origin, dest) => dest.cost;
            Func<Node, double> estimate = n => 0.0; // no useful heuristics, which turns a* into dijkstra's alorithm
            Func<Node, IEnumerable<Node>> neighbours = n => n.Neighbours;

            var p10 = Path<Node>.FindPath(n1, n0, distance, estimate, neighbours);
            Assert.IsNull(p10);

            var p12 = Path<Node>.FindPath(n1, n2, distance, estimate, neighbours);
            Assert.IsNotNull(p12);
            Assert.AreEqual(2, p12.TotalCost);
            CollectionAssert.AreEqual(new[] { n2, n1 }, p12.ToArray());

            var p18 = Path<Node>.FindPath(n1, n8, distance, estimate, neighbours);
            Assert.IsNotNull(p18);
            Assert.AreEqual(13, p18.TotalCost);
            CollectionAssert.AreEqual(new[] { n8, n3, n2, n1 }, p18.ToArray());

            var p43 = Path<Node>.FindPath(n4, n3, distance, estimate, neighbours);
            Assert.IsNotNull(p43);
            Assert.AreEqual(6, p43.TotalCost);
            CollectionAssert.AreEqual(new[] { n3, n2, n1, n4 }, p43.ToArray());

            var p78 = Path<Node>.FindPath(n7, n8, distance, estimate, neighbours);
            Assert.IsNotNull(p78);
            Assert.AreEqual(14, p78.TotalCost);
            CollectionAssert.AreEqual(new[] { n8, n6, n7 }, p78.ToArray());

            n5.cost = 0; // mutate graph so 7->8 yields another path

            p78 = Path<Node>.FindPath(n7, n8, distance, estimate, neighbours);
            Assert.IsNotNull(p78);
            Assert.AreEqual(13, p78.TotalCost);
            CollectionAssert.AreEqual(new[] { n8, n3, n2, n5, n7 }, p78.ToArray());
        }

        class Node
        {
            public Node north;
            public Node west;
            public Node east;
            public Node south;
            public int cost;

            public IEnumerable<Node> Neighbours
            {
                get
                {
                    if (north != null) yield return north;
                    if (east != null) yield return east;
                    if (west != null) yield return west;
                    if (south != null) yield return south;
                }
            }
        }

        [TestMethod]
        public void TestFindPath_AStar()
        {
            const int width = 4, height = 4;
            var map = new int[width * height]
            {
               10, 20, 30, 40,
               21, 31, 41, 11,
               32, 42, 12, 22,
               43, 13, 23, 33,
            };

            Func<int, int, int> nodeAt = (x, y) => map[y * width + x];
            Func<int, Tuple<int, int>> getNodeXY = n =>
            {
                var index = Array.IndexOf(map, n);
                return Tuple.Create(index % width, index / width);
            };
            Func<int, int, double> distanceBetweenNodes = (n1, n2) =>
            {
                var pos1 = getNodeXY(n1);
                var pos2 = getNodeXY(n2);
                var dx = pos1.Item1 - pos2.Item1;
                var dy = pos1.Item2 - pos2.Item2;
                return Math.Sqrt(dx * dx + dy * dy);
            };

            Func<Path<int>, int, double> distance = (origin, dest) => dest;
            Func<int, IEnumerable<int>> neighbours = n =>
            {
                var pos = getNodeXY(n);
                var x = pos.Item1;
                var y = pos.Item2;
                var list = new List<int>();

                if (x > 0 && y > 0)
                    list.Add(nodeAt(x - 1, y - 1));
                if (y > 0)
                    list.Add(nodeAt(x, y - 1));
                if (x < width - 1 && y > 0)
                    list.Add(nodeAt(x + 1, y - 1));

                if (x > 0)
                    list.Add(nodeAt(x - 1, y));
                if (x < width - 1)
                    list.Add(nodeAt(x + 1, y));

                if (x > 0 && y < height - 1)
                    list.Add(nodeAt(x - 1, y + 1));
                if (y < height - 1)
                    list.Add(nodeAt(x, y + 1));
                if (x < width - 1 && y < height - 1)
                    list.Add(nodeAt(x + 1, y + 1));

                return list;
            };

            var p1033 = Path<int>.FindPath(10, 33, distance, n => distanceBetweenNodes(n, 33), neighbours);
            Assert.IsNotNull(p1033);
            Assert.AreEqual(76, p1033.TotalCost);
            CollectionAssert.AreEqual(new[] { 33, 12, 31, 10 }, p1033.ToArray());

            var p3222 = Path<int>.FindPath(32, 22, distance, n => distanceBetweenNodes(n, 22), neighbours);
            Assert.IsNotNull(p3222);
            Assert.AreEqual(47, p3222.TotalCost);
            CollectionAssert.AreEqual(new[] { 22, 12, 13, 32 }, p3222.ToArray());

            var p2230 = Path<int>.FindPath(22, 30, distance, n => distanceBetweenNodes(n, 30), neighbours);
            Assert.IsNotNull(p2230);
            Assert.AreEqual(41, p2230.TotalCost);
            CollectionAssert.AreEqual(new[] { 30, 11, 22 }, p2230.ToArray());

            var p4010 = Path<int>.FindPath(40, 10, distance, n => distanceBetweenNodes(n, 10), neighbours);
            Assert.IsNotNull(p4010);
            Assert.AreEqual(60, p4010.TotalCost);
            CollectionAssert.AreEqual(new[] { 10, 20, 30, 40 }, p4010.ToArray());
        }
    }
}
